home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Libris Britannia 4
/
science library(b).zip
/
science library(b)
/
HAM_RAD
/
3345.ZIP
/
MACPARK.FOR
< prev
next >
Wrap
Text File
|
1988-03-07
|
17KB
|
732 lines
$debug
c FIR linear phase filter design program
c from the book "Digital Filter Design" by Parks & Burris
c they adapted it from EQFIR, written by McLellen & Parks &
c published in the IEEE book "Programs for Digital Signal Processing"
c
c
common pi2,ad,dev,x,y,grid,des,wt,alpha,iext,nfcns,ngrid
common /oops/niter,iout
dimension iext(66),ad(66),alpha(66),x(66),y(66)
dimension des(1045),grid(1045),wt(1045)
dimension edge(20),fx(10),wtx(10),deviat(10)
dimension h(66),hh(132)
double precision pi2,pi,ad,dev,x,y,gee,d
integer bd1,bd2,bd3,bd4
data bd1,bd2,bd3,bd4/1hb,1ha,1hn,1hd/
input=5
iout=3
pi=4.0*datan(1.0d0)
pi2=2.0d0*pi
c the program is set up for a max length of 128, but
c this upper limit can be changed by ensioning the
c arrays iext, ad, alpha, x, y, h to be nfmax/2+2
nfmax=128
100 continue
open(3,file='pm.lst')
open(4,file='r.dat')
jtype=0
c
c input section
c
write(*,104)
104 format(3x,'Enter filter length, type, # of bands')
read(*,*) nfilt,jtype,nbands
if(nfilt.eq.0) stop
if(nfilt.le.nfmax.or.nfilt.ge.3) go to 115
call error
stop
115 if(nbands.le.0) nbands=1
c
c grid density is assumed to be 16
c
lgrid=16
jb=2*nbands
write(*,120)
120 format(' Enter band edges')
read(*,*) (edge(j),j=1,jb)
write(*,121)
121 format(' Enter desired value in each band')
read(*,*) (fx(j),j=1,nbands)
write(*,122)
122 format(' Enter weight in each band')
read(*,*) (wtx(j),j=1,nbands)
if(jtype.gt.0.and.jtype.le.3) go to 125
call error
stop
125 neg=1
if(jtype.eq.1) neg=0
nodd=nfilt/2
nodd=nfilt-2*nodd
nfcns=nfilt/2
if(nodd.eq.1.and.neg.eq.0) nfcns=nfcns+1
c
c set up the dense grid. the number of points in the grid
c is (filter length +1) * grid density / 2
c
grid(1)=edge(1)
delf=lgrid*nfcns
delf=0.5/delf
c write(*,1008) lgrid,nfcns,delf
c1008 format(' lgrid,nfcns,delf=',2i5,f10.6)
if(neg.eq.0) go to 135
if(edge(1).lt.delf) grid(1)=delf
135 continue
j=1
l=1
lband=1
140 fup=edge(l+1)
145 temp=grid(j)
c
c calculate the desired magnitude response and weight
c function on the grid
c
des(j) = eff(temp,fx,wtx,lband,jtype)
wt(j) = wate(temp,fx,wtx,lband,jtype)
j=j+1
grid(j) = temp+delf
if(grid(j).gt.fup) go to 150
go to 145
150 grid(j-1)=fup
des(j-1)=eff(fup,fx,wtx,lband,jtype)
wt(j-1)=wate(fup,fx,wtx,lband,jtype)
lband=lband+1
l=l+2
if(lband.gt.nbands) go to 160
grid(j) = edge(l)
go to 140
160 ngrid=j-1
if(neg.ne.nodd) go to 165
if(grid(ngrid).gt.(0.5-delf)) ngrid=ngrid-1
165 continue
c
c set up a new approximation problem which is equivalent to
c the old problem
c
if(neg) 170,170,180
170 if(nodd.eq.1) go to 200
do 175 j=1,ngrid
change=dcos(pi*grid(j))
des(j)=des(j)/change
175 wt(j)=wt(j)*change
go to 200
180 if(nodd.eq.1) go to 190
do 185 j=1,ngrid
change=dsin(pi*grid(j))
des(j)=des(j)/change
185 wt(j)=wt(j)*change
go to 200
190 do 195 j=1,ngrid
change=dsin(pi2*grid(j))
des(j)=des(j)/change
195 wt(j)=wt(j)*change
c
c initial guess for the extremal frequencies -- equally
c spaced along the grid
c
200 temp=float(ngrid-1)/float(nfcns)
do 210 j=1,nfcns
xt=j-1
210 iext(j)=xt*temp+1.0
iext(nfcns+1)=ngrid
nm1=nfcns-1
nz=nfcns+1
c
c -fa- debug printout of the major arrays that are input to remes
c write(*,1000) (grid(j),j=1,ngrid)
c write(*,1001) (des(j),j=1,ngrid)
c write(*,1002) (wt(j),j=1,ngrid)
c write(*,1003) (iext(j),j=1,nfcns+1)
c1000 format(' GRID = '/,(10f7.4))
c1001 format(' DES = '/,(10f7.4))
c1002 format(' WT = '/,(10f7.4))
c1003 format(' IEXT = '/,(10i7))
c call the remes exchange algorithm to do the approximation
c problem
c
call remes
c
c calculate the impuse response
c
if(neg) 300,300,320
300 if(nodd.eq.0) go to 310
do 305 j=1,nm1
nzmj=nz-j
305 h(j)=0.5*alpha(nzmj)
h(nfcns)=alpha(1)
go to 350
310 h(1)=0.25*alpha(nfcns)
do 315 j=2,nm1
nzmj=nz-j
nf2j=nfcns+2-j
315 h(j)=0.25*(alpha(nzmj)+alpha(nf2j))
h(nfcns)=0.5*alpha(1)+0.25*alpha(2)
go to 350
320 if(nodd.eq.0) go to 330
h(1)=0.25*alpha(nfcns)
h(2)=0.25*alpha(nm1)
do 325 j=3,nm1
nzmj=nz-j
jf3j=nfcns+3-j
325 h(j)=0.25*(alpha(nzmj)-alpha(nf3j))
h(nfcns)=0.5*alpha(1)-0.25*alpha(3)
h(nz)=0.0
go to 350
330 h(1)=0.25*alpha(nfcns)
do 335 j=2,nm1
nzmj=nz-j
nf2j=nfcns+2-j
335 h(j)=0.25*(alpha(nzmj)-alpha(nf2j))
h(nfcns)=0.5*alpha(1)-0.25*alpha(2)
c
c Program output section
c Since iout=3, the output is written to file 'pm.lst'
c
350 write(iout,360)
360 format(1h1,' Finite impulse response (FIR)',/
# ' Linear phase digital filter design',/
# ' Remes exchange algorithm')
if(jtype.eq.1) write(iout,365)
365 format(' Bandpass filter'/)
if(jtype.eq.2) write(iout,370)
370 format(' Differentiator'/)
if(jtype.eq.3) write(iout,375)
375 format(' Hilbert Transformer'/)
write(iout,378) nfilt
378 format(20x,'Filter length =',i3)
c
c for screen output, the impulse response is not written
c
if(iout.eq.6) go to 457
write(iout,380)
380 format(15x, '*** Impulse Response ***')
do 381 j=1,nfcns
k=nfilt-j+1
if(neg.eq.0) write(iout,382) j,h(j),k
if(neg.eq.1) write(iout,383) j,h(j),k
381 continue
382 format(13x,2hh(,i2,4h) = ,e15.8,5h = h(,i3,1h) )
383 format(13x,2hh(,i2,4h) = ,e15.8,6h = -h(,i3,1h) )
if(neg.eq.1.and.nodd.eq.1) write(iout,384) nz
384 format(13x,2hh(,i2,8h) = 0.0 )
c
c now to write impulse response to file 'r.dat'
c write the first half of the response
c
do 785 i=1,nfcns
hh(i)=h(i)
785 continue
c
if(neg.eq.0.and.nodd.eq.0) then
c here neg=0 and nodd=0 for bandpass even length filter
c nfcns=nfilt/2
do 786 n=1,nfcns
hh(nfcns+n)=h(nfcns-n+1)
786 continue
do 787 i=1,nfcns*2
write(4,*) hh(i)
787 continue
c
else if(neg.eq.0.and.nodd.eq.1) then
c here neg=0 and nodd=1 for bandpass odd length filter
c nfcns=nfilt/2+1
do 788 n=1,nfcns-1
hh(nfcns+n)=h(nfcns-n)
788 continue
do 789 i=1,nfcns*2-1
write(4,*) hh(i)
789 continue
c
else if(neg.eq.1.and.nodd.eq.0) then
c neg=1 for diff and hilbert, nodd=0 for even length
do 800 n=1,nfcns
hh(nfcns+n)=-h(nfcns-n+1)
800 continue
do 801 i=1,nfcns*2
write(4,*) hh(i)
801 continue
c
else if(neg.eq.1.and.nodd.eq.1) then
c neg=1 for diff and hilbert, nodd=1 for odd length
h(nfcns+1)=0.
do 802 n=1,nfcns
hh(nfcns+n+1)=-h(nfcns-n+1)
802 continue
do 803 i=1,nfcns*2+1
write(4,*) hh(i)
803 continue
endif
457 do 450 k=1,nbands,4
kup=k+3
if(kup.gt.nbands) kup=nbands
write(iout,385) (bd1,bd2,bd3,bd4,j,j=k,kup)
385 format(24x,4(4a1,i3,7x))
write(iout,390) (edge(2*j-1),j=k,kup)
390 format(2x,'lower band edge',5f14.7)
write(iout,395) (edge(2*j),j=k,kup)
395 format(2x,'upper band edge',5f14.7)
if(jtype.ne.2) write(iout,400) (fx(j),j=k,kup)
400 format(2x,'desired value ',5f14.7)
if(jtype.eq.2) write(iout,405) (fx(j),j=k,kup)
405 format(2x,'desired slope ',5f14.7)
write(iout,410) (wtx(j),j=k,kup)
410 format(2x,'weighting ',5f14.7)
do 420 j=k,kup
420 deviat(j)=dev/wtx(j)
write(iout,425) (deviat(j),j=k,kup)
425 format(2x,'deviation ',5f14.7)
if(jtype.ne.1) go to 450
do 430 j=k,kup
430 deviat(j)=20.0*alog10(deviat(j)+fx(j))
write(iout,435) (deviat(j),j=k,kup)
435 format(2x,'deviation in db',5f14.7)
450 continue
do 452 j=1,nz
ix=iext(j)
452 grid(j)=grid(ix)
c
c extremal frequencies not written out in this version
c
c write(iout,455) (grid(j),j=1,nz)
c455 format(/2x,'extremal frequencies--maxima of the error curve'/
c # (2x,5f14.7))
if(iout.eq.6) go to 461
iout=6
go to 350
461 write(*,462)
462 format(10x,'filter specs are in the file pm.lst')
458 write(*,459)
459 format(10x,'impulse response is in file r.dat')
stop
End
$PAGE
c-----
c Function EFF
c function to calculate the desired magnitude response
c as a function of frequency.
c an arbitrary function of frequency can be
c approximated if the user replaces this function
c with the appropriate code to evaluate the ideal
c magnitude. note that the parameter freq is the value
c of the normalized frequency needed for evaluation.
c-----
function eff(freq,fx,wtx,lband,jtype)
dimension fx(5),wtx(5)
if(jtype.eq.2) go to 1
eff=fx(lband)
return
1 eff=fx(lband)*freq
return
end
c-----
c Function WATE
c function to calculate the weight function as a function of
c frequency. similar to the function eff, this function can
c be replaced by a user-written routine to calculate any
c desired weighting function
c-----
function wate(freq,fx,wtx,lband,jtype)
dimension fx(5),wtx(5)
if(jtype.eq.2) go to 1
wate=wtx(lband)
return
1 if(fx(lband).lt.0.0001) go to 2
wate=wtx(lband)/freq
return
2 wate=wtx(lband)
return
end
c-----
c Subrotine ERROR
c this routine writes an error message if an error detected
c in input data
c-----
subroutine error
common /oops/niter,iout
write(iout,1)
1 format(' Error in input data')
return
end
$PAGE
c-----
c Subroutine REMES
c this routine implements the remes exchange algorithm
c for the weighted chebyshev approximation of a continuous
c function with a sum of consines. inputs to the subroutine
c are a dense grid which replaces the frequency axis, the
c desired funciton on this grid, the weight function on the
c grid, the number of cosines, and an initial guess of the
c extremal frequencies. the program minimizes the chebyshev
c error by determining the best location of the xtremal
c frequencies (points of maximum error) and then calculates
c the coefficients of the best approximation.
c-----
subroutine remes
common pi2,ad,dev,x,y,grid,des,wt,alpha,iext,nfcns,ngrid
common /oops/ niter,iout
dimension iext(66),ad(66),alpha(66),x(66),y(66)
dimension des(1045),grid(1045),wt(1045)
dimension a(66),p(65),q(65)
double precision pi2,dnum,dden,dtemp,a,p,q
double precision dk,dak
double precision ad,dev,x,y
double precision gee,d
c
c the program allows a maximum number of iterations of 25
c
itrmax=25
dev1=-1.0
nz=nfcns+1
nzz=nfcns+2
niter=0
100 continue
iext(nzz)=ngrid+1
niter=niter+1
if(niter.gt.itrmax) go to 400
do 110 j=1,nz
jxt=iext(j)
dtemp=grid(jxt)
dtemp=dcos(dtemp*pi2)
110 x(j)=dtemp
jet=(nfcns-1)/15+1
do 120 j=1,nz
120 ad(j)=d(j,nz,jet)
dnum=0.0
dden=0.0
k=1
do 130 j=1,nz
l=iext(j)
dtemp=ad(j)*des(l)
dnum=dnum+dtemp
dtemp=float(k)*ad(j)/wt(l)
dden=dden+dtemp
130 k=-k
dev=dnum/dden
c intermediate deviations ARE written in this version
write(iout,131) dev
write(*,131) dev
131 format(' deviation = ',f12.9)
nu=1
if(dev.gt.0.0) nu=-1
dev=-float(nu)*dev
k=nu
do 140 j=1,nz
l=iext(j)
dtemp=float(k)*dev/wt(l)
y(j)=des(l)+dtemp
140 k=-k
if(dev.gt.dev1) go to 150
call ouch
go to 400
150 dev1=dev
jchnge=0
k1=iext(1)
knz=iext(nz)
klow=0
nut=-nu
j=1
c
c search for the extremal frequencies of the best
c approximation
c
200 if(j.eq.nzz) ynz=comp
if(j.ge.nzz) go to 300
kup=iext(j+1)
l=iext(j)+1
nut=-nut
if(j.eq.2) y1=comp
comp=dev
if(l.ge.kup) go to 220
err=gee(l,nz)
err=(err-des(l))*wt(l)
dtemp=float(nut)*err-comp
if(dtemp.le.0.0) go to 220
comp=float(nut)*err
210 l=l+1
if(l.ge.kup) go to 215
err=gee(l,nz)
err=(err-des(l))*wt(l)
dtemp=float(nut)*err-comp
if(dtemp.le.0.0) go to 215
comp=float(nut)*err
go to 210
215 iext(j)=l-1
j=j+1
klow=l-1
jchnge=jchnge+1
go to 200
220 l=l-1
225 l=l-1
if(l.le.klow) go to 250
err=gee(l,nz)
err=(err-des(l))*wt(l)
dtemp=float(nut)*err-comp
if(dtemp.gt.0.0) go to 230
if(jchnge.le.0) go to 225
go to 260
230 comp=float(nut)*err
235 l=l-1
if(l.le.klow) go to 240
err=gee(l,nz)
err=(err-des(l))*wt(l)
dtemp=float(nut)*err-comp
if(dtemp.le.0.0) go to 240
comp=float(nut)*err
go to 235
240 klow=iext(j)
iext(j)=l+1
j=j+1
jchnge=jchnge+1
go to 200
250 l=iext(j)+1
if(jchnge.gt.0) go to 215
255 l=l+1
if(l.ge.kup) go to 260
err=gee(l,nz)
err=(err-des(l))*wt(l)
dtemp=float(nut)*err-comp
if(dtemp.le.0.0) go to 255
comp=float(nut)*err
go to 210
260 klow=iext(j)
j=j+1
go to 200
300 if(j.gt.nzz) go to 320
if(k1.gt.iext(1)) k1=iext(1)
if(knz.lt.iext(nz)) knz=iext(nz)
nut1=nut
nut=-nu
l=0
kup=k1
comp=ynz*1.00001
luck=1
310 l=l+1
if(l.ge.kup) go to 315
err=gee(l,nz)
err=(err-des(l))*wt(l)
dtemp=float(nut)*err-comp
if(dtemp.le.0.0) go to 310
comp=float(nut)*err
j=nzz
go to 210
315 luck=6
go to 325
320 if(luck.gt.9) go to 350
if(comp.gt.y1) y1=comp
k1=iext(nzz)
325 l=ngrid+1
klow=knz
nut=-nut1
comp=y1*1.00001
330 l=l-1
if(l.le.klow) go to 340
err=gee(l,nz)
err=(err-des(l))*wt(l)
dtemp=float(nut)*err
if(dtemp.le.0.0) go to 330
j=nzz
comp=float(nut)*err
luck=luck+10
go to 235
340 if(luck.eq.6) go to 370
do 345 j=1,nfcns
nzzmj=nzz-j
nzmj=nz-j
345 iext(nzzmj)=iext(nzmj)
iext(1)=k1
go to 100
350 kn=iext(nzz)
do 360 j=1,nfcns
360 iext(j)=iext(j+1)
iext(nz)=kn
go to 100
370 if(jchnge.gt.0) go to 100
c
c calculation of the coefficients of the best approximation
c using the inverse discrete fourier transform
c
400 continue
nm1=nfcns-1
fsh=1.0e-6
gtemp=grid(1)
x(nzz)=-2.0
cn=2*nfcns-1
delf=1.0/cn
l=1
kkk=0
if(grid(1).lt.0.01.and.grid(ngrid).gt.0.49) kkk=1
if(nfcns.le.3) kkk=1
if(kkk.eq.1) go to 405
dtemp=dcos(pi2*grid(1))
dnum=dcos(pi2*grid(ngrid))
aa=2.0/(dtemp-dnum)
bb=-(dtemp+dnum)/(dtemp-dnum)
405 continue
do 430 j=1,nfcns
ft=j-1
ft=ft*delf
xt=dcos(pi2*ft)
if(kkk.eq.1) go to 410
xt=(xt-bb)/aa
xt1=sqrt(1.0-xt*xt)
ft=atan2(xt1,xt)/pi2
410 xe=x(l)
if(xt.gt.xe) go to 420
if((xe-xt).lt.fsh) go to 415
l=l+1
go to 410
415 a(j)=y(l)
go to 425
420 if((xt-xe).lt.fsh) go to 415
grid(1)=ft
a(j)=gee(1,nz)
425 continue
if(l.gt.1) l=l-1
430 continue
grid(1)=gtemp
dden=pi2/cn
do 510 j=1,nfcns
dtemp=0.0
dnum=j-1
dnum=dnum*dden
if(nm1.lt.1) go to 505
do 500 k=1,nm1
dak=a(k+1)
dk=k
500 dtemp=dtemp+dak*dcos(dnum*dk)
505 dtemp=2.0*dtemp+a(1)
510 alpha(j)=dtemp
do 550 j=2,nfcns
550 alpha(j)=2.0*alpha(j)/cn
alpha(1)=alpha(1)/cn
if(kkk.eq.1) go to 545
p(1)=2.0*alpha(nfcns)*bb+alpha(nm1)
p(2)=2.0*aa*alpha(nfcns)
q(1)=alpha(nfcns-2)-alpha(nfcns)
do 540 j=2,nm1
if(j.lt.nm1) go to 515
aa=.5*aa
bb=.5*bb
515 continue
p(j+1)=0.
do 520 k=1,j
a(k)=p(k)
520 p(k)=2.0*bb*a(k)
p(2)=p(2)+a(1)*2.0*aa
jm1=j-1
do 525 k=1,jm1
525 p(k)=p(k)+q(k)+aa*a(k+1)
jp1=j+1
do 530 k=3,jp1
530 p(k)=p(k)+aa*a(k-1)
if(j.eq.nm1) go to 540
do 535 k=1,j
535 q(k)=-a(k)
nf1j=nfcns-1-j
q(1)=q(1)+alpha(nf1j)
540 continue
do 543 j=1,nfcns
543 alpha(j)=p(j)
545 continue
if(nfcns.gt.3) return
alpha(nfcns+1)=0.0
alpha(nfcns+2)=0.0
return
end
$PAGE
c-----
c function D
c function to calculate the lagrange interpolation
c coefficients for use in the function gee
c-----
c
double precision function d(k,n,m)
common pi2,ad,dev,x,y,grid,des,wt,alpha,iext,nfcns,ngrid
dimension iext(66),ad(66),alpha(66),x(66),y(66)
dimension des(1045),grid(1045),wt(1045)
double precision ad,dev,x,y
double precision q
double precision pi2
d=1.
q=x(k)
do 3 l=1,m
do 2 j=l,n,m
if(j-k) 1,2,1
1 d=2.0*d*(q-x(j))
2 continue
3 continue
d=1.0/d
return
end
c
c-----
c function GEE
c function to evaluate the frequency response using the
c lagrange interpolation formula in the barycentric form
c
double precision function gee(k,n)
common pi2,ad,dev,x,y,grid,des,wt,alpha,iext,nfcns,ngrid
dimension iext(66),ad(66),alpha(66),x(66),y(66)
dimension des(1045),grid(1045),wt(1045)
double precision p,c,d,xf
double precision pi2
double precision ad,dev,x,y
p=0.0
xf=grid(k)
xf=dcos(pi2*xf)
d=0.
do 1 j=1,n
c=xf-x(j)
c=ad(j)/c
d=d+c
1 p=p+c*y(j)
gee=p/d
return
end
c-----
c subroutine OUCH
c writes error message when the algorithm fails to converge
c there seem to be two conditions under which the algorithm
c fails to converge. Initial guess for extremal frequencies
c so poor that the exchange cannot get started, or near termination
c of correct design, deviation decreases due to rounding errors
c and program stops.
c
subroutine ouch
common /oops/niter,iout
write(iout,1) niter
1 format(' ***failure to converge***'/' Probable cause is',
# ' machine rounding error'/' # iterations = ',i5/,
# ' If # iterations exceeds 3, design may be ok,',
# ' but should be checked.')
return
end